# -*- coding: UTF-8 -*-
import almath
from naoqi import *
import time
import  math

ip = "192.168.1.14"
move = ALProxy("ALMotion",ip,9559)
posture = ALProxy("ALRobotPosture",ip,9559)
move.wakeUp()
posture.goToPosture("StandInit",0.3)
time.sleep(2)

try:
    memoryProxy = ALProxy("ALMemory", ip, 9559)
except Exception, e:
    print "Could not create proxy to ALMemory"

    print "Error was: ",


x = 0.2
y = 0.00
theta = 0.0
try:
    move.angleInterpolation("HeadPitch", 0.01, 1, True)
    move.angleInterpolation("HeadYaw", -0.00, 1, False)

    b = 0
    while 1:
        a = input()

        if a == 4:
            b = b-0.3
        if a==6:
            b = b+0.3

        elif a == 8:
            move.move(0.1, b, 0)

        elif a == 1:
            #move.angleInterpolation("HeadPitch", 0.5, 1, True)
            move.angleInterpolation("HeadYaw", 30*almath.TO_RAD, 1, False)
            move.angleInterpolation("HeadYaw", -60 * almath.TO_RAD, 1, False)
            move.angleInterpolation("HeadYaw", 30 * almath.TO_RAD, 1, False)

        elif a == 5:
            move.stopMove()
        else:
            continue
            # move.rest()
            # break45

except Exception, errorMsg:
    print str(errorMsg)
    print "This example is not allowed on this robot."
    exit()
